package legway;

import java.io.File;

import lejos.nxt.*;

/**
 * This class contains the parameters needed to keep the Legway balanced. It contains a PID
 * controller which reads in the angles and the angle velocities from the gyro sensor and
 * the left and right motor, and uses these (weighted) values to calculate the motor output
 * required to keep the Legway balanced. Original code by Steven Witzand
 * 
 * @author Kim Petersen, Andreas Koefoed-Hansen og Tim Rasmussen
 * @version Januar 2010
 * 
 */
public class BalanceController extends Thread
{
	private static final int FALL_ERROR = 8000; // constant for determining when Legway has fallen

	private static MotorController motors;
	private static GyroscopeSensor gyro;

	private static double int_error = 0.0;
	private static double prev_error = 0.0;
	// The PID control parameters
	private final double Kp = 1.2;
	private final double Ki = 0.25;
	private final double Kd = 0.1;
	// Teseing error contributions.
	private final double K_psi = 44.257035; // Gyro angle weight
	private final double K_phi = 0.806876; // Motor angle weight
	private final double K_psidot = 0.620882; // Gyro angle velocity weight
	private final double K_phidot = 0.039711;// Motor angle velocity weight
	private static CtrlParam ctrl;
	public boolean offsetDone = false;

	/**
	 * BalanceController constructor.
	 * 
	 * @param ctrl The motor control parameters.
	 */
	public BalanceController(CtrlParam ctrl) {
		this.ctrl = ctrl;
		motors = new MotorController(Motor.C, Motor.A);
		gyro = new GyroscopeSensor(SensorPort.S3);
		setDaemon(true);
	}

	/**
	 * The BalanceController thread which constantly runs to keep the Legway upright
	 */
	public void run() {
		while (true) {
			delay(6);
			ctrl.runDriveState();
			double Psi = gyro.getAngle();
			double PsiDot = gyro.getAngleVelocity();

			// ctrl.tiltAngle() is used to drive the robot forwards and backwards
			double Phi = motors.getAngle() - ctrl.getTiltAngle();
			double PhiDot = motors.getAngleVelocity();
			// Proportional Error
			double error = Psi * K_psi + Phi * K_phi + PsiDot * K_psidot + PhiDot * K_phidot;
			// Integral Error
			int_error += error;
			// Derivative Error
			double deriv_error = error - prev_error;
			prev_error = error;
			// Power sent to the motors
			double pw = error * Kp + deriv_error * Kd + int_error * Ki;
			if (!testUpright(pw))
				continue;
			motors.setPower(pw + ctrl.getLeftMotorOffset(), pw + ctrl.getRightMotorOffset());
			//LCD.drawString("" + pw, 5, 0);
		}
	}

	/**
	 * Determine whehter the legway is upright or toppled. If it is toppled, 
	 * it resets all necessary parameters and the Legway is ready to go.
	 * @param power 
	 * 			- a double that contains the outout from the PID Controller
	 * @return true if the Legway is upright else false
	 */
	private static boolean testUpright(double power) {
		if (Math.abs(power) < FALL_ERROR)
			return true;

		motors.stop();
		motors.reset();
		File medicFile = new File("medic.wav");
		if(medicFile.canRead())
			Sound.playSample(medicFile);
		
		LCD.drawString("Press ENTER to", 0, 1);
		LCD.drawString("continue", 0, 2);
		Button.ENTER.waitForPressAndRelease();
		LCD.clear();
		int_error = 0;
		prev_error = 0;
		gyro.reset();
		ctrl.resetTiltAngle();
		return false;
	}

	/**
	 * Helping method.
	 * @param time The lenght of the delay
	 */
	public static void delay(int time) {
		try {Thread.sleep(time);} catch (Exception e) {}
	}

}